﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Integrador.Controles
{
    //clase controlador PID para cuadrotor
    //Solo proporcional por ahora
    public class PID
    {
        #region Propiedades

        /// <summary>
        /// constante de proporcionalidad
        /// </summary>
        public double Kp { get; set; }

        /// <summary>
        /// constante de proporcionalidad
        /// </summary>
        public double Kd { get; set; }

        /// <summary>
        /// constante de proporcionalidad
        /// </summary>
        public double Ki { get; set; }

        /// <summary>
        /// Estados anteriores
        /// </summary>
        public double errorAnterior { get; set; }

        public double errorI { get; set; }

        /// <summary>
        /// Tamaño del paso
        /// </summary>
        public double Tp { get; set; }

        #endregion


        #region Constructoras

        public PID()
        {
            this.errorAnterior = 0;
            this.errorI = 0;
        }

        #endregion

        /// <summary>
        /// Control PID
        /// </summary>
        /// <param name="x">estado actual</param>
        /// <param name="xd">estado deseado</param>
        /// <param name="xd">estado de los rotores anterior</param>

        public virtual double[] Control(Estado x, Estado xd, ref double[] RotAnt)
        {
            return null;

        }

        //      previous_error = 0
        //      integral = 0 
        //      start:
        //      error = setpoint - actual_position
        //      integral = integral + (error*dt)
        //      derivative = (error - previous_error)/dt
        //      output = (Kp*error) + (Ki*integral) + (Kd*derivative)
        //      previous_error = error
        //      wait(dt)
        //      goto start


        //La salida de un controlador digital PID se calcula de la siguiente forma:

        //salida = KP+KD+KI

        //Los parametros se calculan de la siguiente forma:

        //P = Valor actual - Valor deseado
        //D = (Valor actual - Valor actual anterior)/tiempo del Loop
        //I = (Valor actual - Valor deseado) * tiempo del Loop + suma de todas la I anteriores

        //Los valores de los factores KP, KI y KD hay que conseguirlos experimentalmente.

    }
}
